﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;

namespace DStarLite
{
    public class DStarLite
    {
        public Dictionary<Point, int> graph;
        private Dictionary<Point, double> g;
        private Dictionary<Point, double> rhs;
        private PriorityQueue<Point> openList;
        public Point start;
        public Point goal;
        private Point last;
        private Dictionary<Point, Point> parent;
        private double km;

        // 八个方向的移动代价
        private static readonly Point[] Directions = {
        new Point(0, 1), new Point(1, 0), new Point(0, -1), new Point(-1, 0),
        new Point(1, 1), new Point(1, -1), new Point(-1, 1), new Point(-1, -1)
    };

        public DStarLite(Dictionary<Point, int> graph, Point start, Point goal)
        {
            this.graph = graph;
            this.start = start;
            this.goal = goal;
            this.last = start;

            Initialize();
        }

        private void Initialize()
        {
            g = new Dictionary<Point, double>();
            rhs = new Dictionary<Point, double>();
            parent = new Dictionary<Point, Point>();
            openList = new PriorityQueue<Point>();
            km = 0;

            // 初始化所有节点的g和rhs值
            foreach (var point in graph.Keys)
            {
                g[point] = double.PositiveInfinity;
                rhs[point] = double.PositiveInfinity;
            }

            rhs[goal] = 0;
            openList.Enqueue(goal, CalculateKey(goal));
        }

        private Key CalculateKey(Point point)
        {
            double minValue = Math.Min(GetGValue(point), GetRhsValue(point));
            double heuristic = Heuristic(point, start);
            return new Key(minValue + heuristic + km, minValue);
        }

        private double GetGValue(Point point)
        {
            return g.ContainsKey(point) ? g[point] : double.PositiveInfinity;
        }

        private double GetRhsValue(Point point)
        {
            return rhs.ContainsKey(point) ? rhs[point] : double.PositiveInfinity;
        }

        private double Heuristic(Point a, Point b)
        {
            // 使用曼哈顿距离作为启发式函数，更稳定
            return Math.Abs(a.X - b.X) + Math.Abs(a.Y - b.Y);
        }
        private double Cost(Point from, Point to)
        {
            // 检查起点和终点是否都可达
            if (!graph.ContainsKey(from) || !graph.ContainsKey(to) ||
                graph[from] < 0 || graph[to] < 0)
                return double.PositiveInfinity;

            double baseCost = Math.Max(graph[to], 1);
            // 对角线移动成本更高
            bool isDiagonal = from.X != to.X && from.Y != to.Y;
            double distance = isDiagonal ? Math.Sqrt(2) : 1.0;

            return baseCost * distance;
        }
        private List<Point> GetPredecessors(Point point)
        {
            var predecessors = new List<Point>();

            foreach (var direction in Directions)
            {
                var pred = new Point(point.X - direction.X, point.Y - direction.Y);
                if (graph.ContainsKey(pred))
                {
                    predecessors.Add(pred);
                }
            }

            return predecessors;
        }
        private List<Point> GetSuccessors(Point point)
        {
            var successors = new List<Point>();

            foreach (var direction in Directions)
            {
                var successor = new Point(point.X + direction.X, point.Y + direction.Y);
                if (graph.ContainsKey(successor))
                {
                    successors.Add(successor);
                }
            }
            return successors;
        }
        private void UpdateVertex(Point u)
        {
            if (u != goal)
            {
                var predecessors = GetPredecessors(u);
                double minRhs = double.PositiveInfinity;
                Point bestPred = u;

                foreach (var pred in predecessors)
                {
                    double cost = Cost(pred, u);  // 注意方向：pred -> u
                    if (cost < double.PositiveInfinity)
                    {
                        double totalCost = GetGValue(pred) + cost;
                        if (totalCost < minRhs)
                        {
                            minRhs = totalCost;
                            bestPred = pred;
                        }
                    }
                }
                rhs[u] = minRhs;
                parent[u] = bestPred; // 记录父节点用于路径重建
            }

            openList.Remove(u);

            if (Math.Abs(GetGValue(u) - GetRhsValue(u)) > 1e-9)
            {
                openList.Enqueue(u, CalculateKey(u));
            }
        }
        private void ComputeShortestPath()
        {
            int maxIterations = graph.Count * 10;
            int iterations = 0;

            while (openList.Count > 0 && iterations < maxIterations)
            {
                iterations++;

                var current = openList.Dequeue().Element;
                Key currentKey = CalculateKey(current);
                Key startKey = CalculateKey(start);

                if (currentKey.CompareTo(startKey) >= 0 && GetRhsValue(start) == GetGValue(start))
                    break;

                if (GetGValue(current) > GetRhsValue(current))
                {
                    // 过度一致
                    g[current] = GetRhsValue(current);
                    foreach (var pred in GetPredecessors(current))  // 更新前驱节点
                    {
                        UpdateVertex(pred);
                    }
                }
                else
                {
                    // 欠一致
                    g[current] = double.PositiveInfinity;
                    foreach (var pred in GetPredecessors(current))  // 更新前驱节点
                    {
                        UpdateVertex(pred);
                    }
                    UpdateVertex(current);
                }
            }

            if (iterations >= maxIterations)
                Console.WriteLine("警告: 达到最大迭代次数");
        }
        public List<Point> FindPath()
        {
            ComputeShortestPath();

            // 标准路径重建：通过最小化 g(s) + cost(current, s) 选择下一节点
            var path = new List<Point>();
            var current = start;

            int maxSteps = graph.Count * 2; // 防止无限循环
            int steps = 0;

            while (current != goal && steps < maxSteps)
            {
                path.Add(current);
                steps++;

                var successors = GetSuccessors(current);
                Point next = current;
                double minCost = double.PositiveInfinity;

                foreach (var succ in successors)
                {
                    double cost = Cost(current, succ);
                    if (cost < double.PositiveInfinity)
                    {
                        double totalCost = GetGValue(succ) + cost;
                        if (totalCost < minCost)
                        {
                            minCost = totalCost;
                            next = succ;
                        }
                    }
                }

                if (minCost >= double.PositiveInfinity)
                {
                    Console.WriteLine("路径重建失败：找不到可达的后继节点");
                    break;
                }

                current = next;

                // 防止循环
                if (path.Contains(current))
                {
                    Console.WriteLine("检测到循环路径");
                    break;
                }
            }

            if (current == goal)
            {
                path.Add(goal);
                Console.WriteLine($"找到路径，长度: {path.Count}");
            }
            else
            {
                Console.WriteLine($"路径重建未完成，当前路径长度: {path.Count}");
            }

            return path;
        }
        public void UpdateGraph(Dictionary<Point, int> changedCells)
        {
            // 更新km值
            km += Heuristic(last, start);
            last = start;

            Console.WriteLine($"地图更新: {changedCells.Count} 个单元格发生变化");

            // 更新图并处理变化的单元格
            foreach (var cell in changedCells)
            {
                int oldValue = graph.ContainsKey(cell.Key) ? graph[cell.Key] : -1;
                graph[cell.Key] = cell.Value;

                Console.WriteLine($"单元格 ({cell.Key.X}, {cell.Key.Y}) 从 {oldValue} 变为 {cell.Value}");

                // 标准D* Lite：更新变化顶点及其所有后继节点
                UpdateVertex(cell.Key);
                foreach (var succ in GetSuccessors(cell.Key))
                {
                    UpdateVertex(succ);
                }
            }

            ComputeShortestPath();
        }
        public void UpdateCurrentPosition(Point newPosition)
        {
            if (!graph.ContainsKey(newPosition) || graph[newPosition] < 0)
            {
                Console.WriteLine("警告: 新位置不可达");
                return;
            }

            // 更新km值：累计从last到start的启发式成本
            km += Heuristic(last, start);

            // 更新last和start位置
            last = start;
            start = newPosition;

            Console.WriteLine($"位置更新: {last} -> {start}, km: {km}");

            // 重新计算路径
            ComputeShortestPath();
        }
        public Point GetCurrentPosition()
        {
            return start;
        }
        public List<Point> GetNextStep()
        {
            var nextStep = new List<Point>();
            nextStep.Add(start);

            if (start != goal)
            {
                var successors = GetSuccessors(start);
                Point bestSucc = start;
                double minCost = double.PositiveInfinity;

                foreach (var succ in successors)
                {
                    double cost = Cost(start, succ);
                    if (cost < double.PositiveInfinity)
                    {
                        double totalCost = GetGValue(succ) + cost;
                        if (totalCost < minCost)
                        {
                            minCost = totalCost;
                            bestSucc = succ;
                        }
                    }
                }

                if (minCost < double.PositiveInfinity)
                {
                    nextStep.Add(bestSucc);
                }
            }

            return nextStep;
        }
        // 调试方法：打印关键节点信息
        public void PrintDebugInfo()
        {
            Console.WriteLine($"起点: ({start.X},{start.Y}), 终点: ({goal.X},{goal.Y})");
            Console.WriteLine($"起点g值: {GetGValue(start):F2}, rhs值: {GetRhsValue(start):F2}");
            if (parent.ContainsKey(start))
            {
                Console.WriteLine($"起点父节点: ({parent[start].X},{parent[start].Y})");
            }
        }
    }
    // 添加Key结构体用于双键优先级比较
    public struct Key : IComparable<Key>
    {
        public double K1;
        public double K2;

        public Key(double k1, double k2)
        {
            K1 = k1;
            K2 = k2;
        }

        public int CompareTo(Key other)
        {
            if (Math.Abs(K1 - other.K1) > 1e-9)
                return K1.CompareTo(other.K1);
            return K2.CompareTo(other.K2);
        }

        public override string ToString()
        {
            return $"({K1:F2}, {K2:F2})";
        }
    }
    public class PriorityQueue<TElement>
    {
        private readonly List<(TElement Element, Key Priority)> elements = new List<(TElement, Key)>();
        private readonly Dictionary<TElement, int> elementIndices = new Dictionary<TElement, int>();

        public int Count => elements.Count;

        public void Enqueue(TElement item, Key priority)
        {
            if (elementIndices.ContainsKey(item))
            {
                Remove(item);
            }

            elements.Add((item, priority));
            elementIndices[item] = elements.Count - 1;
            HeapifyUp(elements.Count - 1);
        }

        public (TElement Element, Key Priority) Dequeue()
        {
            if (elements.Count == 0)
                throw new InvalidOperationException("Queue is empty");

            var result = elements[0];
            elementIndices.Remove(result.Element);

            if (elements.Count > 1)
            {
                elements[0] = elements[elements.Count - 1];
                elementIndices[elements[0].Element] = 0;
            }

            elements.RemoveAt(elements.Count - 1);

            if (elements.Count > 0)
            {
                HeapifyDown(0);
            }

            return result;
        }

        public (TElement Element, Key Priority) Peek()
        {
            if (elements.Count == 0)
                throw new InvalidOperationException("Queue is empty");
            return elements[0];
        }

        public bool Remove(TElement item)
        {
            if (!elementIndices.ContainsKey(item))
                return false;

            int index = elementIndices[item];
            elementIndices.Remove(item);

            if (index == elements.Count - 1)
            {
                elements.RemoveAt(elements.Count - 1);
            }
            else
            {
                elements[index] = elements[elements.Count - 1];
                elementIndices[elements[index].Element] = index;
                elements.RemoveAt(elements.Count - 1);
                HeapifyDown(index);
            }

            return true;
        }

        public bool Contains(TElement item)
        {
            return elementIndices.ContainsKey(item);
        }

        private void HeapifyUp(int index)
        {
            while (index > 0)
            {
                int parentIndex = (index - 1) / 2;
                if (elements[index].Priority.CompareTo(elements[parentIndex].Priority) >= 0)
                    break;

                Swap(index, parentIndex);
                index = parentIndex;
            }
        }

        private void HeapifyDown(int index)
        {
            while (true)
            {
                int leftChildIndex = index * 2 + 1;
                int rightChildIndex = index * 2 + 2;
                int smallestIndex = index;

                if (leftChildIndex < elements.Count &&
                    elements[leftChildIndex].Priority.CompareTo(elements[smallestIndex].Priority) < 0)
                {
                    smallestIndex = leftChildIndex;
                }

                if (rightChildIndex < elements.Count &&
                    elements[rightChildIndex].Priority.CompareTo(elements[smallestIndex].Priority) < 0)
                {
                    smallestIndex = rightChildIndex;
                }

                if (smallestIndex == index)
                    break;

                Swap(index, smallestIndex);
                index = smallestIndex;
            }
        }

        private void Swap(int i, int j)
        {
            var temp = elements[i];
            elements[i] = elements[j];
            elements[j] = temp;
            elementIndices[elements[i].Element] = i;
            elementIndices[elements[j].Element] = j;
        }
    }
}